// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HeartbeatAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.EV4.Runtime
{  
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;   
    using System.IO;
    using System.Runtime.Serialization;
    using System.Timers;
    using System.Xml;
    using System.Xml.Serialization;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.ManagedBulkUsb;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    
    /// <summary>
    /// Heartbeat producer agent publishing a variety of sensor data from the EV4
    /// </summary>
    [System.Diagnostics.CodeAnalysis.SuppressMessage(
        "Microsoft.Design",
        "CA1001:TypesThatOwnDisposableFieldsShouldBeDisposable",
        Justification = "Agents dispose of resources in <c>OnDeactived</c> method.")]
    [DataContract]
    public class HeartbeatAgent : Agent, 
                                  IMessagePublisher<AccelerometerMessage>, 
                                  IMessagePublisher<BatteryMessage>, 
                                  IMessagePublisher<ButtonMessage>, 
                                  IMessagePublisher<CompassMessage>, 
                                  IMessagePublisher<DifferentialDriveMessage>, 
                                  IMessagePublisher<EncoderMessage>, 
                                  IMessagePublisher<FanMessage>, 
                                  IMessagePublisher<GyroscopeMessage>, 
                                  IMessagePublisher<HumidityMessage>, 
                                  IMessagePublisher<IRProximityMessage>, 
                                  IMessagePublisher<JointMessage>, 
                                  IMessagePublisher<PowerMessage>, 
                                  IMessagePublisher<ProjectorMessage>, 
                                  IMessagePublisher<SonarMessage>, 
                                  IMessagePublisher<TemperatureMessage>, 
                                  IMessagePublisher<IntegerErrorMessage>,
                                  IControlReceiver<DifferentialDriveSetVelocityMessage>, 
                                  IControlReceiver<SetTurretPanMessage>,
                                  IControlReceiver<SetServoSequenceMessage>, 
                                  IObserver<HeartbeatState>
    {
        /// <summary>
        /// Default drive command renewal period in ms
        /// </summary>
        private const double DefaultDriveCommandRenewalPeriod = 500;

        /// <summary>
        /// Duration to wait between sending a HostHeartbeat to the drive board
        /// </summary>
        private const int HostHeartbeatPeriodMS = 50;

        /// <summary>
        /// A forwarder that can be used to broadcast Accelerometers.
        /// </summary>
        private IForwarder<AccelerometerMessage> accelerometerPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast Battery state.
        /// </summary>
        private IForwarder<BatteryMessage> batteryPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast ButtonState.
        /// </summary>
        private IForwarder<ButtonMessage> buttonPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast CompassState.
        /// </summary>
        private IForwarder<CompassMessage> compassPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast DifferentialDriveState.
        /// </summary>
        private IForwarder<DifferentialDriveMessage> drivePublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast EncoderState.
        /// </summary>
        private IForwarder<EncoderMessage> encoderPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast FanState.
        /// </summary>
        private IForwarder<FanMessage> fanPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast GyroscopeState.
        /// </summary>
        private IForwarder<GyroscopeMessage> gyroscopePublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast HumidityState.
        /// </summary>
        private IForwarder<HumidityMessage> humidityPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast IRProximityState.
        /// </summary>
        private IForwarder<IRProximityMessage> proximityPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast JointState.
        /// </summary>
        private IForwarder<JointMessage> jointPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast PowerState.
        /// </summary>
        private IForwarder<PowerMessage> powerPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast ProjectorState.
        /// </summary>
        private IForwarder<ProjectorMessage> projectorPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast SonarState.
        /// </summary>
        private IForwarder<SonarMessage> sonarPublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast TemperatureState.
        /// </summary>
        private IForwarder<TemperatureMessage> temperaturePublisher;

        /// <summary>
        /// A forwarder that can be used to broadcast ErrorMessages.
        /// </summary>
        private IForwarder<IntegerErrorMessage> errorPublisher;

        /// <summary>
        /// The IDisposable we use to stop receiving state updates
        /// </summary>
        private IDisposable unsubscribe;

        /// <summary>
        /// The drive board
        /// </summary>
        private DriveBoard driveBoard;

        /// <summary>
        /// Timer used for checking to see if we need to stop the robot
        /// </summary>
        private Timer watchdogTimer;

        /// <summary>
        /// The physical configuration of our device
        /// </summary>
        private DeviceConfiguration config;

        /// <summary>
        /// The turret configuration for ev4.
        /// </summary>
        private TurretConfig turretConfig;

        /// <summary>
        /// Robot configuration folder, which contains any robot specific information.
        /// </summary>
        [DataMember]
        private string robotConfigFolder;

        /// <summary>
        /// Initializes a new instance of the <see cref="HeartbeatAgent"/> class. 
        /// </summary>
        /// <param name="name">
        /// The name
        /// </param>
        /// <param name="driveCommandRenewalPeriod">
        /// Grace period, in milliseconds, we will allow between drive commands.
        /// </param>
        /// <param name="robotConfigFolder">Robot configuration folder, which contains any robot specific customizations</param>
        public HeartbeatAgent(string name, double driveCommandRenewalPeriod, string robotConfigFolder)
            : base(name)
        {
            this.robotConfigFolder = robotConfigFolder;
            this.DriveCommandRenewalPeriod = driveCommandRenewalPeriod;
        }

        /// <summary>
        /// Gets or sets the grace period, in milliseconds, we will allow between drive commands.
        /// If a drive command is not received in this period, we will stop the robot
        /// </summary>
        [DataMember]
        public double DriveCommandRenewalPeriod { get; set; }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<AccelerometerMessage> forwarder)
        {
            this.accelerometerPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<BatteryMessage> forwarder)
        {
            this.batteryPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<ButtonMessage> forwarder)
        {
            this.buttonPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<CompassMessage> forwarder)
        {
            this.compassPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<DifferentialDriveMessage> forwarder)
        {
            this.drivePublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<EncoderMessage> forwarder)
        {
            this.encoderPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<FanMessage> forwarder)
        {
            this.fanPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<GyroscopeMessage> forwarder)
        {
            this.gyroscopePublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<HumidityMessage> forwarder)
        {
            this.humidityPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<IRProximityMessage> forwarder)
        {
            this.proximityPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<JointMessage> forwarder)
        {
            this.jointPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<PowerMessage> forwarder)
        {
            this.powerPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<ProjectorMessage> forwarder)
        {
            this.projectorPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<SonarMessage> forwarder)
        {
            this.sonarPublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<TemperatureMessage> forwarder)
        {
            this.temperaturePublisher = forwarder;
        }

        /// <summary>
        /// Initializer that provides a way for us to broadcast messages.
        /// </summary>
        /// <param name="forwarder">A forwarder that can be used to broadcast our state.</param>
        public void InitializePublisher(IForwarder<IntegerErrorMessage> forwarder)
        {
            this.errorPublisher = forwarder;
        }

        /// <summary>
        /// Called when Heartbeat agent is activated
        /// </summary>
        public override void OnActivated()
        {
            if (this.DriveCommandRenewalPeriod == 0)
            {
                this.DriveCommandRenewalPeriod = DefaultDriveCommandRenewalPeriod;
            }

            // Create a transport
            ITransport transport;
            transport = new Device("b35924d6-3e16-4a9e-9782-5524a4b79bac");

            // Read in device configuration
            StreamReader stringReader;
            stringReader = new StreamReader("EV4.config.xml");
            XmlTextReader xmlReader;
            xmlReader = new XmlTextReader(stringReader);
            XmlSerializer xserial = new XmlSerializer(typeof(DeviceConfiguration));
            this.config = (DeviceConfiguration)xserial.Deserialize(xmlReader);

            this.turretConfig = new TurretConfig(0, 0);
            if (this.robotConfigFolder != null)
            {
                string turretConfigFilePath = Path.Combine(this.robotConfigFolder, TurretConfig.Filename);
                TurretConfig turretConfigFromConfigFile = DataLoader<TurretConfig>.ImportData(turretConfigFilePath);
                if (this.turretConfig == null)
                {
                    TraceOut.Error(TraceContexts.HW, "Turret configuration not found at {0}", turretConfigFilePath);
                    Console.WriteLine("Turret configuration not found at {0}", turretConfigFilePath);
                }
                else
                {
                    this.turretConfig = turretConfigFromConfigFile;
                    TraceOut.Info(TraceContexts.HW, "Loaded Turret configuration at {0} with pan calib angle = {1} tilt calib angle = {2}", turretConfigFilePath, this.turretConfig.PanOffsetInRadians, this.turretConfig.TiltOffsetInRadians);
                    Console.WriteLine("Loaded Turret configuration at {0} with pan calib angle = {1} tilt calib angle = {2}", turretConfigFilePath, this.turretConfig.PanOffsetInRadians, this.turretConfig.TiltOffsetInRadians);
                }
            }
            else
            {
                TraceOut.Warning(TraceContexts.HW, "Robot config folder not specified, not loading turret configuration. Using default values. This will result in bad slam, nav performance");
                Console.WriteLine("Robot config folder not specified, not loading turret configuration. Using default values. This will result in bad slam, nav performance");
            }

            // Create the driveBoard proxy
            this.driveBoard = new DriveBoard(
                this.config, transport, new Version2Protocol(transport), HostHeartbeatPeriodMS);

            // Subscribe to updates from the DriveBoard and store the unsubscribe for later
            this.unsubscribe = this.driveBoard.Subscribe(this);

            // Initialize the DriveBoard
            this.driveBoard.InitializeDriveBoard();

            // Launch the stopping watchdog
            this.SetupStoppingWatchdog();

            base.OnActivated();
        }

        /// <summary>
        /// Called when Heartbeat agent is deactivated
        /// </summary>
        public override void OnDeactivated()
        {
            this.unsubscribe.Dispose();
            this.driveBoard.Dispose();
            this.watchdogTimer.Dispose();
            base.OnDeactivated();
        }

        /// <summary>
        /// Notifies the observer that the provider has finished sending push-based notifications. 
        /// </summary>
        public void OnCompleted()
        {
            throw new Exception("EV4 has indicated that it is no longer sending us updates");
        }

        /// <summary>
        ///  Notifies the observer that the provider has experienced an error condition.
        /// </summary>
        /// <param name="error">An object that provides additional information about the error.</param>
        public void OnError(Exception error)
        {
            FirmwareException firmwareError = (FirmwareException)error;
            if (firmwareError != null)
            {
                this.errorPublisher.Post(new IntegerErrorMessage(firmwareError.ResponseCode, AgentMessage.GetCurrentTime()));
            }
        }

        /// <summary>
        /// Called when a new HeartbeatState is ready
        /// </summary>
        /// <param name="state">Latest HeartbeatState</param>
        public void OnNext(HeartbeatState state)
        {
            if (state.Accelerometers != null && state.Accelerometers.Count > 0)
            {
                this.accelerometerPublisher.Post(new AccelerometerMessage(state.Accelerometers, state.Timestamp));
            }

            if (state.Batteries != null && state.Batteries.Count > 0)
            {
                this.batteryPublisher.Post(new BatteryMessage(state.Batteries, state.Timestamp));
            }

            if (state.Buttons != null && state.Buttons.Count > 0)
            {
                this.buttonPublisher.Post(new ButtonMessage(state.Buttons, state.Timestamp));
            }

            if (state.Compasses != null && state.Compasses.Count > 0)
            {
                this.compassPublisher.Post(new CompassMessage(state.Compasses, state.Timestamp));
            }

            if (state.DifferentialDrives != null && state.DifferentialDrives.Count > 0)
            {
                this.drivePublisher.Post(new DifferentialDriveMessage(state.DifferentialDrives, state.Timestamp));
            }

            if (state.Encoders != null && state.Encoders.Count > 0)
            {
                this.encoderPublisher.Post(new EncoderMessage(state.Encoders, state.Timestamp));
            }

            if (state.Fans != null && state.Fans.Count > 0)
            {
                this.fanPublisher.Post(new FanMessage(state.Fans, state.Timestamp));
            }

            if (state.Gyroscopes != null && state.Gyroscopes.Count > 0)
            {
                this.gyroscopePublisher.Post(new GyroscopeMessage(state.Gyroscopes, state.Timestamp));
            }

            if (state.HumiditySensors != null && state.HumiditySensors.Count > 0)
            {
                this.humidityPublisher.Post(new HumidityMessage(state.HumiditySensors, state.Timestamp));
            }

            if (state.Joints != null && state.Joints.Count > 0)
            {
                this.UpdatePanTiltWithRobotSpecificTurretConfig(state.Joints);

                this.jointPublisher.Post(new JointMessage(state.Joints, state.Timestamp));
            }

            if (state.PowerDevices != null && state.PowerDevices.Count > 0)
            {
                this.powerPublisher.Post(new PowerMessage(state.PowerDevices, state.Timestamp));
            }

            if (state.Projectors != null && state.Projectors.Count > 0)
            {
                this.projectorPublisher.Post(new ProjectorMessage(state.Projectors, state.Timestamp));
            }

            if (state.InfraredSensors != null && state.InfraredSensors.Count > 0)
            {
                this.proximityPublisher.Post(new IRProximityMessage(state.InfraredSensors, state.Timestamp));
            }

            if (state.Sonars != null && state.Sonars.Count > 0)
            {
                this.sonarPublisher.Post(new SonarMessage(state.Sonars, state.Timestamp));
            }

            if (state.TemperatureSensors != null && state.TemperatureSensors.Count > 0)
            {
                this.temperaturePublisher.Post(new TemperatureMessage(state.TemperatureSensors, state.Timestamp));
            }
        }

        /// <summary>
        /// Sets drive velocities
        /// </summary>
        /// <param name="setMessage">The message containing the SetDriveVelocities command</param>
        public void ReceiveControl(DifferentialDriveSetVelocityMessage setMessage)
        {
            if (this.driveBoard != null)
            {
                // Reset the watchdog timer to stop this drive command if nothing else comes in
                this.watchdogTimer.Stop();
                this.watchdogTimer.Start();
                SetDriveVelocities driveVelocities = new SetDriveVelocities(
                    setMessage.State.LeftVelocity, setMessage.State.RightVelocity);

                // Send the command
                this.driveBoard.DispatchCommand(driveVelocities);
            }
        }

        /// <summary>
        /// Sets turret pan
        /// </summary>
        /// <param name="setMessage">The message containing the set turret pan request</param>
        public void ReceiveControl(SetTurretPanMessage setMessage)
        {
            double desiredPositionInRadians = setMessage.Radians + this.turretConfig.PanOffsetInRadians;
            if (desiredPositionInRadians < this.config.Turret.MinPositionInRadians
                || desiredPositionInRadians > this.config.Turret.MaxPositionInRadians)
            {
                throw new ArgumentException(
                    string.Format(
                        "Invalid position [{0:0.000}] Must be between [{1:0.000}] and [{2:0.000}]",
                        desiredPositionInRadians, 
                        this.config.Turret.MinPositionInRadians, 
                        this.config.Turret.MaxPositionInRadians));
            }

            if (this.driveBoard != null)
            {
                // Send the command
                this.driveBoard.DispatchCommand(new SetTurretPosition((float)desiredPositionInRadians));
            }
        }

        /// <summary>
        /// Sets servo sequence
        /// </summary>
        /// <param name="setMessage">The message containing the set turret pan request</param>
        public void ReceiveControl(SetServoSequenceMessage setMessage)
        {
            unsafe
            {
                if (this.driveBoard != null)
                {
                    // Send the command
                    const int PanServoId = 2;
                    MotionSequenceParams motionSequenceParams = new MotionSequenceParams();
                    motionSequenceParams.Id = PanServoId;
                    motionSequenceParams.IsStartOfSequence = (byte)1;
                    motionSequenceParams.SequenceLength = 1;

                    // The firmware expects positions in milliradians.
                    motionSequenceParams.Positions[0] = (int)((setMessage.Radians + this.turretConfig.PanOffsetInRadians) * MathConstants.MilliunitsPerUnit);
                    motionSequenceParams.Times[0] = setMessage.Duration;

                    this.driveBoard.DispatchCommand(new SetServoSequence(motionSequenceParams));
                }
            }
        }

        /// <summary>
        /// Updates the pan tilt information with robot specific calibration offset angles.
        /// </summary>
        /// <param name="joints">Turret pan tilt joints</param>
        private void UpdatePanTiltWithRobotSpecificTurretConfig(List<JointState> joints)
        {
            const int TiltServoIndex = 0;
            const int PanServoIndex = 1;

            joints[TiltServoIndex].PositionInRadians -= this.turretConfig.TiltOffsetInRadians;
            joints[PanServoIndex].PositionInRadians -= this.turretConfig.PanOffsetInRadians;
        }

        /// <summary>
        /// Launches a timer that monitors active drive commands and issues a stop when we have not received a drive command within the 
        /// specified renewal time.
        /// </summary>
        private void SetupStoppingWatchdog()
        {
            this.watchdogTimer = new Timer(this.DriveCommandRenewalPeriod);
            this.watchdogTimer.Elapsed += new ElapsedEventHandler(this.HandleStopTimer);
            this.watchdogTimer.AutoReset = false;
        }

        /// <summary>
        /// Checks to see if we need to issue a stop command due to a move command expiring without another move command being received
        /// </summary>
        /// <param name="sender">The parameter is not used.</param>
        /// <param name="e">The parameter is not used.</param>
        private void HandleStopTimer(object sender, ElapsedEventArgs e)
        {
            // Caller ran out of time to request another velocity. We are stopping
            this.driveBoard.DispatchCommand(new SetDriveVelocities(0.000f, 0.000f));
        }
    }
}
